using System;
using System.Text;
using Microsoft.SPOT;

namespace IMU6DOFV2.Netduino
{
    public class IMUData
    {
        #region Members
        internal bool satFixed = false;
        internal int numSatellites = 0;
        private int imuH = 0;
        private double roll = 0.0;
        private double pitch = 0.0;
        private double yaw = 0.0;
        private double latitude;
        private double altitude;
        private double longitude;
        private double groundVel = 0.0;
        private string version; 
        #endregion

        #region Properties
        public double Roll
        {
            get { return roll; }
            set { roll = value; }
        }

        public double Pitch
        {
            get { return pitch; }
            set { pitch = value; }
        }

        public double Yaw
        {
            get { return yaw; }
            set { yaw = value; }
        }

        public double Latitude
        {
            get { return latitude; }
            set { latitude = value; }
        }

        public double Altitude
        {
            get { return altitude; }
            set { altitude = value; }
        }

        public double Longitude
        {
            get { return longitude; }
            set { longitude = value; }
        }

        public double GroundVel
        {
            get { return groundVel; }
            set { groundVel = value; }
        }

        public string Version
        {
            get { return version; }
            set { version = value; }
        }

        public int ImuH 
        { 
            get {   return imuH;    }
            set {   imuH = value;   }
        }

        public int GPSQuality
        {
            get { return 100; }
        }
        #endregion

        #region Overrided Methods
        public override string ToString()
        {
            // !!!VER:1.8.1,RLL:-0.01,PCH:-0.02,YAW:1.98,IMUH:253,TOW:0***
            string res = string.Empty;
            res += "!!!VER:" + version;
            res += ",RLL:" + roll;
            res += ",PCH:" + pitch;
            res += ",YAW:" + yaw;
            res += ",IMUH:0";
            res += ",TOW:0***";
            return res;
        }
	    #endregion        
        
        public static IMUData TryParse(string sentence)
        {
            IMUData res = new IMUData();

            string tempStr;

            try
            {

                sentence = sentence.Trim();
                sentence = sentence.Substring(sentence.LastIndexOf('!') + 1);

                // !!!VER:1.8.1,RLL:-0.01,PCH:-0.02,YAW:1.98,IMUH:253,TOW:0***
                // !!!VER:1.8.1,RLL:0.00,PCH:0.04,YAW:-0.29,IMUH:253,LAT:0,LON:0,ALT:0,COG:0,SOG:0,FIX:0,SAT:0,TOW:100923938

                string[] partialSentences = sentence.Split(',');
                res.version = partialSentences[0].Split(':')[1];
                tempStr = partialSentences[1].Split(':')[1];
                res.Roll = tempStr.Substring(0, 1) == "-" ? -Convert.ToDouble(tempStr) : Convert.ToDouble(tempStr);
                tempStr = partialSentences[2].Split(':')[1];
                res.Pitch = tempStr.Substring(0, 1) == "-" ? -Convert.ToDouble(tempStr) : Convert.ToDouble(tempStr);
                tempStr = partialSentences[3].Split(':')[1];
                res.Yaw = tempStr.Substring(0, 1) == "-" ? -Convert.ToDouble(tempStr) : Convert.ToDouble(tempStr);
                tempStr = partialSentences[4].Split(':')[1];
                res.imuH = Convert.ToInt16(tempStr);

                if (partialSentences.Length > 6)
                {
                    //tempStr = partialSentences[5].Split(':')[1];
                    //res.latitude = Convert.ToInt16(tempStr);
                    tempStr = partialSentences[6].Split(':')[1];
                    res.longitude = Convert.ToInt16(tempStr);
                    tempStr = partialSentences[7].Split(':')[1];
                    res.altitude = Convert.ToInt16(tempStr);
                    tempStr = partialSentences[10].Split(':')[1];
                    res.satFixed = Convert.ToInt16(tempStr) == 0 ? false : true;
                    tempStr = partialSentences[11].Split(':')[1];
                    res.numSatellites = Convert.ToInt16(tempStr);
                }
            }
            catch (Exception ex)
            {
                Debug.Print("Error en el procesamiento de datos de entrada\r\n");
            }
            return res;
        }
    }
}
